import math


def calculate_avg_speed(car_data, meter, target_lon, target_lat):
    """
    计算车辆在距离目标点5米范围内的平均速度

    参数:
        car_data (dict): 车辆的所有数据，格式为 {ts1: data1, ts2: data2, ...}
        target_lon (float): 目标点经度
        target_lat (float): 目标点纬度

    返回:
        tuple: (车辆ID, 平均速度) 或 None（如果没有符合条件的点）
    """
    # 1. 筛选距离 <5 米的点
    valid_points = []

    for ts, point_data in car_data.items():
        lon = point_data["lon"]
        lat = point_data["lat"]
        distance = haversine(lon, lat, target_lon, target_lat)
        if distance < meter:  # 单位：米
            valid_points.append(point_data)
            break

    # 2. 如果没有符合条件的点，返回 None
    if not valid_points:
        return None

    # 3. 计算平均速度（单位：km/h）
    avg_speed = sum(p["speed"] for p in valid_points) / len(valid_points)
    car_id = valid_points[0]["id"]  # 所有点的 car_id 相同
    lon = valid_points[0]["lon"]
    lat = valid_points[0]["lat"]
    plate = point_data["plate"]
    sn_id = point_data["sn_id"]
    c_type = point_data["type"]
    return (car_id, avg_speed, lon, lat, sn_id, plate, c_type)


def is_vehicle_passed_gantry(vehicle_lon, vehicle_lat, vehicle_heading, gantry_lon, gantry_lat, distance_threshold=30,
                             angle_threshold=45):
    """
    判断车辆是否经过门架
    :param vehicle_lat: 车辆纬度
    :param vehicle_lon: 车辆经度
    :param vehicle_heading: 车辆航向角（度，从正北顺时针方向）
    :param gantry_lat: 门架纬度
    :param gantry_lon: 门架经度
    :param distance_threshold: 距离阈值（米），默认100米
    :param angle_threshold: 角度阈值（度），默认30度
    :return: True（经过）或False（未经过）
    """
    # 计算车辆与门架的距离
    distance = haversine(vehicle_lon, vehicle_lat, gantry_lon, gantry_lat)
    if distance > distance_threshold:
        return False

    # 计算车辆到门架的方位角
    bearing = haversine(vehicle_lon, vehicle_lat, gantry_lon, gantry_lat)

    # 计算车辆航向与方位角的夹角
    angle_diff = abs((vehicle_heading - bearing + 180) % 360 - 180)
    if angle_diff <= angle_threshold:
        return True
    else:
        return False


def haversine(lon1, lat1, lon2, lat2):
    """
    计算两点之间的球面距离（单位：米）
    :param lon1: 点1经度
    :param lat1: 点1纬度
    :param lon2: 点2经度
    :param lat2: 点2纬度
    :return: 距离（单位：米）
    """
    # 将经纬度从度转换为弧度
    lon1, lat1, lon2, lat2 = map(math.radians, [lon1, lat1, lon2, lat2])

    # Haversine公式
    dlon = lon2 - lon1
    dlat = lat2 - lat1
    a = math.sin(dlat / 2) ** 2 + math.cos(lat1) * math.cos(lat2) * math.sin(dlon / 2) ** 2
    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

    # 地球半径（单位：米）
    R = 6371000
    distance_meters = R * c
    return distance_meters


def calculate_catch_up_time(
        v_A0,  # 车辆A的初始速度 (m/s)
        v_B,  # 车辆B的速度 (恒定, m/s)
        a_A,  # 车辆A的加速度 (m/s²)
        n,  # 车辆A提速百分比 (%)
        lon_A, lat_A,  # 车辆A的经纬度
        lon_B, lat_B  # 车辆B的经纬度
):
    """
    计算车辆A提速后追上车辆B的时间。

    返回:
        (加速时间, 匀速时间):
            - 如果在加速阶段追上，匀速时间为0。
            - 如果在匀速阶段追上，返回实际的匀速时间。
            - 如果无法追上，匀速时间为None。
    """
    # 参数验证
    if a_A <= 0:
        raise ValueError("加速度必须为正数")
    if n <= 0:
        raise ValueError("提速百分比必须为正数")

    # 计算初始距离
    D0 = haversine(lon_A, lat_A, lon_B, lat_B)

    # 1. 计算提速后速度
    v_A1 = v_A0 * (1 + n / 100)

    # 2. 计算加速时间
    t_acc = (v_A1 - v_A0) / a_A

    # 3. 计算加速阶段行驶距离（A车）
    D_acc_A = v_A0 * t_acc + 0.5 * a_A * t_acc ** 2

    # 4. 计算加速阶段B车行驶距离
    D_acc_B = v_B * t_acc

    # 5. 计算加速结束时两车的距离
    D_after_acc = D0 + D_acc_B - D_acc_A

    # 检查是否在加速阶段追上
    # 解方程: 0.5*a_A*t^2 + (v_A0 - v_B)*t - D0 = 0
    a = 0.5 * a_A
    b = v_A0 - v_B
    c = -D0
    discriminant = b ** 2 - 4 * a * c

    if discriminant >= 0:
        sqrt_discriminant = math.sqrt(discriminant)
        t1 = (-b + sqrt_discriminant) / (2 * a)
        t2 = (-b - sqrt_discriminant) / (2 * a)
        valid_times = [t for t in [t1, t2] if 0 <= t <= t_acc]
        # 在加速阶段追上
        if valid_times:
            t_catch = min(valid_times)
            return (t_catch, 0)

            # 检查是否在匀速阶段追上
    # 在匀速阶段追上
    if v_A1 > v_B and D_after_acc > 0:
        t_const = D_after_acc / (v_A1 - v_B)
        return (t_acc, t_const)

        # 无法追上
    return (t_acc, None)


def calculate_catch_down_time(
        v_A0,  # 车辆A的初始速度 (m/s)
        v_B,  # 车辆B的速度 (恒定, m/s)
        a_A,  # 车辆A的减速度 (m/s², 正值)
        n,  # 车辆A减速百分比 (%)
        lon_A, lat_A,  # 车辆A的经纬度
        lon_B, lat_B  # 车辆B的经纬度
):
    """
    计算车辆B追上减速的车辆A的时间。

    返回:
        (减速时间, 匀速时间):
            - 如果在减速阶段追上，匀速时间为0。
            - 如果在匀速阶段追上，返回实际的匀速时间。
            - 如果无法追上，匀速时间为None。
    """
    # 计算初始距离
    D0 = haversine(lon_A, lat_A, lon_B, lat_B)

    # 车辆A减速后的速度
    v_A1 = v_A0 * (1 - n / 100)

    # 减速时间
    t_dec = (v_A1 - v_A0) / (-a_A)

    # 减速阶段行驶距离
    D_dec = v_A0 * t_dec + 0.5 * (-a_A) * t_dec ** 2

    # 检查是否在减速阶段追上
    # 解方程: 0.5*(-a_A)*t^2 + (v_A0 - v_B)*t - D0 = 0
    a = 0.5 * (-a_A)
    b = v_A0 - v_B
    c = -D0
    discriminant = b ** 2 - 4 * a * c

    if discriminant >= 0:
        sqrt_discriminant = math.sqrt(discriminant)
        t1 = (-b + sqrt_discriminant) / (2 * a)
        t2 = (-b - sqrt_discriminant) / (2 * a)
        valid_times = [t for t in [t1, t2] if 0 <= t <= t_dec]

        if valid_times:
            t_catch = min(valid_times)
            return (t_catch, 0)  # 在减速阶段追上

    # 检查是否在匀速阶段追上
    if v_A1 < v_B:
        t_const = (D0 + v_A1 * t_dec - D_dec) / (v_B - v_A1)
        if t_const > t_dec:
            return (t_dec, t_const)  # 在匀速阶段追上

    # 无法追上
    return (t_dec, None)


if __name__ == '__main__':
    # 示例数据
    car_data = {
        1747901766472: {"id": 100436, "sn_id": "15_100436", "lon": 107.66953893572278, "lat": 29.371276877454,
                        "speed": 8},
        1747901766575: {"id": 100436, "sn_id": "15_100436", "lon": 107.66953146617716, "lat": 29.371280580840825,
                        "speed": 8.3333},
        1747901766676: {"id": 100436, "sn_id": "15_100436", "lon": 107.66952399663104, "lat": 29.371284284227237,
                        "speed": 8.3333}
    }

    # 目标点坐标（假设为某个路口中心）
    target_lon = 107.66953
    target_lat = 29.37128

    # 调用函数
    result = calculate_avg_speed(car_data, 5, target_lon, target_lat)
    car_id, avg_speed, lon, lat = result
    print(car_id, avg_speed, lon, lat)  # 输出: (100436, 8.2222)
